# ------------------------------------------------------------------
# konrad.smolinski@gmail.com
#
# date: 		15/01/2010
# last update: 	03/03/2010
#
# info:
#	
# 03/03/2011
#	[+] fixed bug at lvlFind2D - stopping rule
#		[+] rounding error :  rderr <- 0.0005
#	[+] structure of the code changed: I am rotating Cartesian system now
#
# TODO : 
#	[+] Bisection search (done)
#	[+] Validate exit rule (done)
#	[-] dependence between initial point and initial search-direction

# ------------------------------------------------------------------
lvlFind2D <- function(pt=c(0.6,0,6),criterion=NULL,step=c(0.01,0.01),rderr=0.005,plt=F){

	if(length(pt)!=2) stop("Initial point has to be 2D.")
	if(is.null(criterion)) stop("Criterion function is required.")

	# search step (accuracy / lattice density)
	stx <- step[1]	  	# X-direction
	sty <- step[2]		# Y-direction

	# hit the boundary and get ready: propose intial point 'ptNow'
	ptIni <- lvlHit2D(pt,criterion=criterion,step=c(0,step[2]),plt=plt)
	check <- "right" 	# initialize direction of the search
	ptNow <- ptIni		
	ptMat <- NULL		# boundary points of the level set (initialize)

	tst <- c(FALSE,FALSE,FALSE)	# test points ahead (Q) have I been here before?
	GO <- TRUE					# Green light to GO and search

	while(GO){

		switch(check,
			right = { # Search direction: to the East (right)
				checkRight <- "down"
				checkLeft <- "up"
				
				lookAhead <- c(stx,0)
				lookRight <- c(stx,-sty)
				lookLeft <- c(stx,sty)
				
				moveAhead <- c(stx,0)
				moveRight <- c(0,-sty)
				moveLeft <- c(0,sty)				
								
			},  # end RIGHT
			up = { # Search direction: to the North (up)
				checkRight <- "right"
				checkLeft <- "left"
				
				lookAhead <- c(0,sty)
				lookRight <- c(stx,sty)
				lookLeft <- c(-stx,sty)

				moveAhead <- c(0,sty)
				moveRight <- c(stx,0)
				moveLeft <- c(-stx,0)
				
			},  # end UP
			
			left = { # Search direction: to the West (left)
				checkRight <- "up"
				checkLeft <- "down"
				
				lookAhead <- c(-stx,0)
				lookRight <- c(-stx,sty)
				lookLeft <- c(-stx,-sty)

				moveAhead <- c(-stx,0)
				moveRight <- c(0,sty)
				moveLeft <- c(0,-sty)
			},  # end LEFT
			
			down = { # Search direction: to the South (down)			
				checkRight <- "left"
				checkLeft <- "right"
				
				lookAhead <- c(0,-sty)
				lookRight <- c(-stx,-sty)
				lookLeft <- c(stx,-sty)
				
				moveAhead <- c(0,-sty)
				moveRight <- c(-stx,0)
				moveLeft <- c(stx,0)
			} # end DOWN

		)  # end SWITCH

		ptMat <- rbind(ptMat,ptNow) 	# append result:

		# SEARCH
		ptCheck <- ptNow + lookRight
		if(!criterion(ptCheck)){
			ptNow <- ptNow + moveRight
			check <- checkRight
		}else{
			ptCheck <- ptNow + lookLeft			
			if(criterion(ptCheck)){ 	
				ptNow <- ptNow + moveLeft
				check <- checkLeft
			}else{  ptNow <- ptNow + moveAhead }	
		} # end ELSE

		
		# STOPPING RULE
		tst[1] <- (sqrt(sum((ptIni-(ptNow + lookAhead))^2)) < rderr)
		tst[2] <- (sqrt(sum((ptIni-(ptNow + lookRight))^2)) < rderr)
		tst[3] <- (sqrt(sum((ptIni-(ptNow + lookLeft ))^2)) < rderr)
		if(any(tst)) GO <- FALSE
		
		#  PLOT
		if(plt){
			points(ptIni[1],ptIni[2],col=3,cex=0.4)
			points(ptNow[1],ptNow[2],col=3,cex=0.4)
			if(any(tst)) 	points(ptNow[1],ptNow[2],col=2,cex=0.4)
		}
	}
	res <- ptMat
return(res)
}
# --- EOF ----------------------------------------------------------